package com.rockwell.robot2015.model.states.running.sequence;

import java.util.ArrayList;

import com.rockwell.robot2015.model.MazeType;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.solvers.IMazeSolver;
import com.rockwell.robot2015.model.solvers.LeftFirstMazeSolver;
import com.rockwell.robot2015.model.solvers.RightFirstMazeSolver;
import com.rockwell.robot2015.model.solvers.SuperMazeSolver;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

/**
 * Navigating in the maze
 * 
 * @author XWu1
 * 
 */
public class FindingDropAreaSequence extends RobotRunningSequence {
	static MazeType m_mazeType = MazeType.None;
	static boolean m_justStarted = false;
	
	static IMazeSolver[] m_solvers = new IMazeSolver[] {
		new LeftFirstMazeSolver(),
		new RightFirstMazeSolver()
	};
	
	static IMazeSolver m_solver;
	
	static int m_retriesAtDeadend;
	
	public enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start finding drop treasure", 2);

				//m_mazeType = checkMaze(machine);
				
				/*
				if (m_mazeType == 3) {
					if (machine.robot().getPredefinedMazeSolver() != null) {
						m_solver = machine.robot().getPredefinedMazeSolver();
					}
					else {
						m_solver = new LoopMazeSolver(false);
					}
				}
				else if (m_mazeType > 0){
					m_solver = m_solvers[m_mazeType-1];
				}
				else {
					m_solver = new LoopMazeSolver(false);
				}
				*/
				
				if (machine.robot().getCurrentMaze() == MazeType.Maze3) {
					m_solver = new SuperMazeSolver(false);
				}
				else {
					m_solver = new LeftFirstMazeSolver();
				}
				
				m_solver.reset(machine.robot());
				
				m_justStarted = true;
				m_retriesAtDeadend = 0;
				
				machine.robot().getSensor().setInMaze(true);

				machine.robot().log("Maze solver type: " + m_solver.toString(), 2);
				machine.changeState(DetectDropArea);
			}

			@Override
			public void exit(IStateMachine machine) {
			}

		},
		DetectDropArea {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				if (detectDropArea(machine)) {
					machine.changeState(DropAreaFound);
				}
				else {
					IRobotSensors sensor = robot.getSensor();
					
					if (sensor.detectFrontWall()) {
						machine.changeState(FrontWallDetected);
					}
					else if (sensor.detectLeftWall() && sensor.detectRightWall()) {
						machine.changeState(JogInWall);
					}
					else {
						machine.changeState(JunctionReached);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		JogInWall {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();

				robot.log("Jogging in walls", 4);

				/*
				if (m_justStarted || !robot.isJogging3()) {
					m_justStarted = false;
					machine.robot().startJogging3();
				}
				*/
				// m_history.add(RobotAction.Jog);
			}

			@Override
			public void move(IStateMachine machine) {
				// machine.robot().sendCommand(RobotAction.Jog);
				// m_history.add(RobotAction.Jog);
				Robot robot = machine.robot();

				if (robot.getCurrentMaze() == MazeType.Maze3) {
					robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
					
					robot.sendCommand(RobotAction.MoveWithSensor, 30);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					machine.changeState(DetectDropArea);
				}
				else {
					robot.sendCommand(RobotAction.Jog3);
					
					/*
					IDetector detector = robot.getDropAreaDetector();
					if (detector.isPresent()) {
						machine.changeState(DropAreaFound);
						return;
					}
					 */
					
					// IDetector detector_entranceguide =
					// robot.getMazeEntranceGuideDetector();
	
					IRobotSensors sensor = robot.getSensor();
	
					/*
					 * if (detector_entranceguide.isPresent() && m_mazeType == 3) {
					 * solver = 1; }
					 */
	
					if (robot.isJogDone()) {
						if (sensor.detectLeftWall() == false || sensor.detectRightWall() == false) {
							machine.changeState(JunctionReached);
						} else if (sensor.detectFrontWall() == true) {
							machine.changeState(FrontWallDetected);
						}
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		OutOfJunction {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();

				robot.log("Out of Junction", 4);
				//robot.startJogging2();
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				boolean dropAreaDetected = detectDropArea(machine);

				IRobotSensors sensor = robot.getSensor();
								
				if (robot.getCurrentMaze() == MazeType.Maze3) {
					robot.sendCommand(RobotAction.MoveWithSensor, 30);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				else {
					robot.sendCommand(RobotAction.Jog2);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					robot.sendCommand(RobotAction.MoveWithSensor, IMazeSolver.AFTER_JUNCTION);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				
				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				
				/*
				if (robot.getCurrentMaze() == MazeType.Maze3) {
					if (sensor.detectLeftWall() && !sensor.detectRightWall())
				}
				*/
				/*
				IDetector detector = robot.getDropAreaDetector();
				if (detector.isPresent()) {
					machine.changeState(DropAreaFound);
				}
				else {
					machine.changeState(JogInWall);
				}*/

				if (dropAreaDetected) {
					machine.changeState(DropAreaFound);
				}
				else {
					dropAreaDetected = detectDropArea(machine);

					if (dropAreaDetected) {
						machine.changeState(DropAreaFound);
					}
					else {
						if (!sensor.detectFrontWall() && !sensor.detectLeftWall() && !sensor.detectRightWall()) {
							// We are out of the maze
							machine.changeState(OutOfMaze);
						}
						else {
							machine.changeState(DetectDropArea);
						}
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		JunctionReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Junction reached", 4);
				
				m_solver.onJunctionReached(machine.robot());
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				if (robot.getCurrentMaze() != MazeType.Maze3) {
					robot.sendCommand(RobotAction.MoveWithSensor, IMazeSolver.JUNCTION_CLEARANCE);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				
				if (detectDropArea(machine)) {
					/*
					robot.sendCommand(RobotAction.Jog2);
					
					robot.sendCommand(RobotAction.MoveWithSensor, 10);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					machine.changeState(DropAreaFound);
					*/
					machine.changeState(OutOfJunction);
				}
				else {
					m_solver.move(robot);
				}
				
				/*
				IDetector detector = robot.getDropAreaDetector();
				if (detector.isPresent()) {
					machine.changeState(DropAreaFound);
				}
				else {
					machine.changeState(OutOfJunction);
				}*/
				
				machine.changeState(OutOfJunction);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}
		},
		FrontWallDetected {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Front wall detected", 4);
				
				m_solver.onFrontWallReached(machine.robot());
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();

				if (robot.getCurrentMaze() != MazeType.Maze3) {
					robot.sendCommand(RobotAction.MoveWithoutSensor, IMazeSolver.JUNCTION_CLEARANCE);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				
				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IRobotSensors sensor = robot.getSensor();
				
				boolean hasLeftWall = sensor.detectLeftWall();
				boolean hasRightWall = sensor.detectRightWall();
				boolean hasFrontWall = sensor.detectFrontWall();
				
				String log = "LeftSensor: " + hasLeftWall + ", RightSensor: " + hasRightWall + ", FrontSensor: " + hasFrontWall;
				robot.log(log, 4);

				if (sensor.detectFrontWall() && sensor.detectLeftWall() && sensor.detectRightWall()) {
					machine.changeState(DeadendReached);
				}
				else {
					m_solver.move(robot);
					machine.changeState(OutOfJunction);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		DeadendReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Deadend reached", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				m_retriesAtDeadend++;
				
				if (m_retriesAtDeadend > 3) {
					machine.changeState(DropAreaFound);
				}
				else {
					robot.sendCommand(RobotAction.MoveToTreasure, -5);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					if (detectDropArea(machine)) {
						robot.sendCommand(RobotAction.MoveToTreasure, 5);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						machine.changeState(DropAreaReached);
					}
					else {
						robot.sendCommand(RobotAction.MoveToTreasure, 5);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						machine.changeState(DetectDropArea);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		DropAreaFound {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.log("Drop area found", 2);

				//if (!robot.isJogging3()) {
				//	machine.robot().startJogging3();
				//}
			}

			@Override
			public void move(IStateMachine machine) {
				/*
				 * int backPos = 0;
				 * 
				 * while (true) { backPos = findbackPos(m_history, 0);
				 * System.out.println("Here " + backPos);
				 * 
				 * while (backPos != -1) { checkSequence(m_history, backPos);
				 * backPos = findbackPos(m_history, backPos - 1); }
				 * 
				 * backPos = findbackPos(m_history, 0);
				 * 
				 * // System.out.println(backPos); if (backPos == -1) { //
				 * System.out.println("I am out!"); break; } }
				 */

				Robot robot = machine.robot();

				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				//robot.sendCommand(RobotAction.Jog3);
				robot.sendCommand(RobotAction.MoveWithoutSensor, 50);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IRobotSensors sensor = robot.getSensor();
				
				boolean hasLeftWall = sensor.detectLeftWall();
				boolean hasRightWall = sensor.detectRightWall();
				boolean hasFrontWall = sensor.detectFrontWall();
				
				String log = "LeftSensor: " + hasLeftWall + ", RightSensor: " + hasRightWall + ", FrontSensor: " + hasFrontWall;
				robot.log(log, 4);

				if (sensor.detectFrontWall() && sensor.detectLeftWall() && sensor.detectRightWall()) {
					machine.changeState(DropAreaReached);
				}
				else {
					machine.changeState(JunctionReached);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		OutOfMaze {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Out of maze without dropping treasure", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		DropAreaReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Drop area reached", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		};

		static FindingDropAreaSequence m_sequence;

		public static void setSequence(FindingDropAreaSequence sequence) {
			m_sequence = sequence;
		}
	}

	private static boolean detectDropArea(IStateMachine machine) {
		Robot robot = machine.robot();
		
		IDetector detector = robot.getDropAreaDetector();
		if (detector.isPresent()) {
			int angle = RobotRunningSequence.getOrientation(robot, detector);
			
			if (Math.abs(angle) < 20) {
				return true;
			}
		}
		
		return false;
	}
	
	private static int findbackPos(ArrayList<RobotAction> m_history, int prevPos) {
		for (int i = prevPos; i < m_history.size(); i++) {
			if (m_history.get(i).equals(RobotAction.TurnBack)) {
				return i;
			}
		}
		return -1;
	}

	private static void checkSequence(ArrayList<RobotAction> m_history,
			int backPos) {
		ArrayList<RobotAction> seqArr = new ArrayList<RobotAction>();
		for (int seq = 1; seq < 7; seq++) {
			switch (seq) {
			case 1:
				seqArr.clear();
				seqArr.add(RobotAction.TurnLeft);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.TurnLeft);
				break;
			case 2:
				seqArr.clear();
				seqArr.add(RobotAction.TurnRight);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.TurnLeft);
				break;
			case 3:
				seqArr.clear();
				seqArr.add(RobotAction.TurnLeft);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				break;
			case 4:
				seqArr.clear();
				seqArr.add(RobotAction.TurnLeft);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.TurnRight);
				break;
			case 5:
				seqArr.clear();
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.TurnLeft);
				break;
			case 6:
				seqArr.clear();
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				seqArr.add(RobotAction.Jog);
				break;
			}

			boolean verifiedSeq = false;
			verifiedSeq = ((m_history.get(backPos - 2) == seqArr.get(0))
					&& (m_history.get(backPos - 1) == seqArr.get(1))
					&& (m_history.get(backPos + 1) == seqArr.get(2)) && (m_history
					.get(backPos + 2) == seqArr.get(3)));

			if (verifiedSeq == true) {
				switch (seq) {
				case 1:
					m_history.remove(backPos + 2);
					m_history.remove(backPos + 1);
					m_history.remove(backPos);
					m_history.remove(backPos - 1);
					m_history.remove(backPos - 2);
					break;

				case 3:
				case 5:
					m_history.set(backPos, RobotAction.TurnRight);
					m_history.remove(backPos + 2);
					// m_history.remove(backPos+1);
					m_history.remove(backPos - 1);
					m_history.remove(backPos - 2);
					break;

				case 2:
				case 4:
					m_history.remove(backPos + 2);
					m_history.remove(backPos + 1);
					m_history.remove(backPos - 1);
					m_history.remove(backPos - 2);
					break;

				case 6:
					m_history.remove(backPos + 1);
					m_history.remove(backPos - 1);
					break;
				}
			}
		}
	}

	public FindingDropAreaSequence(RobotRunningStateMachine running) {
		super(running, State.Start);

		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		// robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(false);
		robot.enableDropAreaDetector(true);
		robot.enableOuterWallDetector(false);
	}

	@Override
	public void reset() {
		super.reset();
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.DropAreaReached);
	}

	@Override
	public boolean failed() {
		return m_currState.equals(State.OutOfMaze);
	}
	
	public void setMazeType(MazeType type) {
		m_mazeType = type;
	}
	
	public MazeType getMazeType() {
		return m_mazeType;
	}
}
